/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvWorld/geometry.h
 * @Description  : 该文件定义了几何相关的类和枚举，包括点、角度、区域、姿态、速度、移动方向和旋转方向等。 
 *                 提供了用于几何计算和变换的各种方法和运算符重载。  
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 15:42:36
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#pragma once

#include <cmath>
#include "z_types.h"
#include "tools.h"

// The interface of class "Point".
class Point {
 public:
	// The constructor
	Point(float x, float y) : x_(x), y_(y) {}

	// Default constructor
	Point() : x_(0.0f), y_(0.0f) {}

	// Get the point object
	Point& GetPntObject() { return *this; }

	// Overloaded operator "=="
	bool operator==(const Point& pt) const {
		return (x_ == pt.x_ && y_ == pt.y_);
	}

	// Overloaded operator "!="
	bool operator!=(const Point& pt) const {
		return (x_ != pt.x_ || y_ != pt.y_);
	}

	// Get the distance to another point
	float DistanceTo(const Point& pt) const {
		return std::sqrt(Square(x_ - pt.x_) + Square(y_ - pt.y_));
	}

 public:
	float x_;  // X coordinates
	float y_;  // Y coordinates
};

// The interface of class "Angle".
enum class AngleMode {
	InRadian,
	InDegree,
};

class Angle {
 public:
	// Constructors
	Angle(float val, AngleMode mode = AngleMode::InRadian);
	Angle(): radian_(0.0f) {}
	Angle(const Point& pt1, const Point& pt2);

	// Convert degree into radian
	static float ToRadian(float deg);

	// Convert radian into degree
	static float ToDegree(float rad);

	// Normalize an angle
	static float NormAngle(float rad);

	// Normalize an angle
	static float NormAngle2(float rad);

	/**
	 * Returns an equivalent angle to the first parameter such that the difference
	 * when the first parameter is subtracted from this new value is an angle
	 * in the normalized range of [-pi, pi], i.e. abs(minuend - subtrahend) <= pi.
	 * @param subtrahend
	 * @param minuend
	 * @return normalized subtrahend angle
	 */
	static double NormalizeAngleDifference(double subtrahend, double minuend);

	// Set the resolution for angle computation
	static float SetReso(float reso);

	// Get the angle value in degree
	float Degree() const;

	// Return the angle's Quadrant
	int Quadrant() const;

	// Rotate the angle
	void Rotate(float angle);

	// Normalize the angle
	float NormAngle();

	// Normalize the angle
	float NormAngle2();

	// Overloaded operators
	Angle operator-() const;
	Angle operator!() const;
	Angle operator+(const Angle& ang) const;
	Angle operator-(const Angle& ang) const;
	void operator+=(const Angle& ang);
	void operator-=(const Angle& ang);
	bool operator==(const Angle& ang) const;
	bool operator!=(const Angle& ang) const;
	bool operator>(const Angle& ang) const;
	bool operator<(const Angle& ang) const;
	bool operator>=(const Angle& ang) const;
	bool operator<=(const Angle& ang) const;
	void operator=(float rad);
	Angle operator+(float rad) const;
	Angle operator-(float rad) const;
	void operator+=(float rad);
	void operator-=(float rad);
	bool operator==(float rad) const;
	bool operator!=(float rad) const;
	bool operator>(float rad) const;
	bool operator<(float rad) const;
	bool operator>=(float rad) const;
	bool operator<=(float rad) const;
	bool ApproxEqualTo(const Angle& ang, float max_diff_rad = 0) const;
	float GetDifference(const Angle& another) const;
	bool InRange(const Angle& ang1, const Angle& ang2) const;

 public:
	float radian_;  			// The angle value in radian
	static float resolution_;   // Resolution for angle computation
};

float sin(const Angle& ang);
float cos(const Angle& ang);
float tan(const Angle& ang);
Angle abs(const Angle& ang);
float AngleDiff(float angle1, float angle2);
float NormAngle2(float rad);
bool LessDegree(const Angle& src_angle, float degree);

// The interface of class "Region".
class Region {
 public:
  // The constructor
	Region(float left, float top, float right, float bottom)
		: left_(left), top_(top), right_(right), bottom_(bottom) {}

	// Default constructor
	Region() : left_(0.0f), top_(0.0f), right_(0.0f), bottom_(0.0f) {}

	// Get the width of the region
	float Width() const { return (right_ - left_); }

	// Get the height of the region
	float Height() const { return (top_ - bottom_); }

	// Whether the region contains the specified point
	bool Contain(const Point& pt) const {
		return (pt.x_ >= left_ && pt.x_ <= right_ && pt.y_ >= bottom_ && pt.y_ <= top_);
	}

	// Whether the region contains the specified point (form#2)
	bool Contain(float x, float y) const {
		return (x >= left_ && x <= right_ && y >= bottom_ && y <= top_);
	}

 public:
	float left_;
	float top_;
	float right_;
	float bottom_;
};

// The interface of class "Posture".
class Posture : public Point {
 public:
	// Constructors
	Posture(float x, float y, float angle) : Point(x, y), theta_(angle) {}
	Posture(float x, float y, const Angle& angle)
		: Point(x, y), theta_(angle.radian_) {}
	Posture(const Point& pt, const Angle& angle)
		: Point(pt), theta_(angle.radian_) {}

	// Default constructor
	Posture() : Point(), theta_(0.0f) {}

	void SetPnt(const Point& pt) {
		x_ = pt.x_;
		y_ = pt.y_;
	}

	void SetPnt(float x, float y) {
		x_ = x;
		y_ = y;
	}

	void SetAngle(const Angle& ang) {
		theta_ = ang.radian_;
	}

	void SetPosture(const Point& pt, const Angle& ang) {
		SetPnt(pt);
		SetAngle(ang);
	}

	void SetPosture(float x, float y, float angle) {
		x_ = x;
		y_ = y;
		theta_ = Angle::NormAngle(angle);
	}
	
	void SetAngle(float angle) {
		theta_ = Angle::NormAngle(angle);
	}

	Angle GetAngle() const {
		return Angle(theta_);
	}

 public:
	float theta_;              // The direction angle
};

// The interface of class "Velocity".
class Velocity {
 public:
	// The constructor
	Velocity(float linear, float angular)
		: vel_x_(linear), vel_y_(0.0f), linear_(linear), angular_(angular) {}

	// Default constructor
	Velocity() : vel_x_(0.0f), vel_y_(0.0f), linear_(0.0f), angular_(0.0f) {}

	void SetZero() {
		vel_x_ = 0.0f;
		vel_y_ = 0.0f;
		linear_ = 0.0f;
		angular_ = 0.0f;
	}

 public:
	float vel_x_;     // X direction velocity component in the body coordinate system
	float vel_y_;     // Y direction velocity component in the body coordinate system
	float linear_;    // Scalar of the velocity at the center of the body
	float angular_;   // Angular velocity around the Z-axis in the body coordinate system

};

// The interface of class "MoveDir".
enum class MoveDirTag {
	Forward,
	Backward,
	Leftward = 0,
	Rightward = 1
};

class MoveDir {
 public:
	MoveDirTag move_dir_tag_;  // The tag for the move direction

	// The constructor
	MoveDir(MoveDirTag move_dir_tag) : move_dir_tag_(move_dir_tag) {}

	// Default constructor
	MoveDir() : move_dir_tag_(MoveDirTag::Forward) {}

	// Copy constructor
	void operator=(const MoveDir& move_dir);

	// Assignment of move direction
	void operator=(MoveDirTag move_dir_tag);

	// Test if 2 objects are identical
	bool operator==(const MoveDir& move_dir) const;

	// Test if 2 objects are not identical
	bool operator!=(const MoveDir& move_dir) const;

	// Test if the object is of the specified turn direction
	bool operator==(MoveDirTag move_dir_tag) const;

	// Test if the object is not of the specified turn direction
	bool operator!=(MoveDirTag move_dir_tag) const;

	// Get the opposite turn direction
	MoveDir operator!() const;
};

// The interface of class "TurnDir".
enum class TurnDirTag {
	CounterClockwise,
	Clockwise
};

class TurnDir {
 public:
	TurnDirTag turn_dir_tag_;  // The tag for the turn direction

	// The constructor
	TurnDir(TurnDirTag turn_dir_tag) : turn_dir_tag_(turn_dir_tag) {}

	// Default constructor
	TurnDir() : turn_dir_tag_(TurnDirTag::CounterClockwise) {}

	// Copy constructor
	void operator=(const TurnDir& turn_dir);

	// Assignment of turn direction
	void operator=(TurnDirTag turn_dir_tag);

	// Test if 2 objects are identical
	bool operator==(const TurnDir& turn_dir) const;

	// Test if 2 objects are not identical
	bool operator!=(const TurnDir& turn_dir) const;

	// Test if the object is of the specified turn direction
	bool operator==(TurnDirTag turn_dir_tag) const;

	// Test if the object is not of the specified turn direction
	bool operator!=(TurnDirTag turn_dir_tag) const;

	// Get the opposite turn direction
	TurnDir operator!() const;
};

// The interface of class "Transform".
struct LocalFrame {
	Point origin_;     // The origin point of the local frame
	Angle slant_;      // The slant angle of the local frame
};

class Transform {
 public:
	// The constructor
	Transform(LocalFrame& local_frame);

	// Overloaded constructor
	Transform(const Point& origin, const Angle& slant);

	// Default constructor
	Transform() {}

	// Init the origin and slant angle of the local frame
	void Init(Posture& local_posture);

	// A transformation from the local frame to the world frame
	Point& GetWorldPoint(const Point& local_point);

	// A reverse transformation from the world frame to the local frame
	Point& GetLocalPoint(const Point& world_point);

	// A transformation from the local frame to the world frame
	Posture& GetWorldPosture(Posture& local_posture);

	// A reverse transformation from the world frame to the local frame
	Posture& GetLocalPosture(Posture& world_posture);
 private:
	Point origin_;     // The origin point of the local frame
	Angle slant_;      // The slant angle of the local frame
	Point point_;      // Buffer for point transformation
	Posture posture_;  // Buffer for posture transformation
};

// The interface of class "Line".
class Line {
 public:
	// Constructor form #1
	Line(const Point& start_point, const Point& end_point);

	// Constructor form #2
	Line(const Point& start_point, const Angle& slant_angle, float total_length);

	// Constructor form #3
	Line(Posture& start_posture, float total_length);

	// The default constructor
	Line() {}

	// Return to starting point
	Point& GetStartPoint() { return start_point_; }

	// Return to end point
	Point& GetEndPoint() { return end_point_; }

	// Get the size of the line
	float Length() const;

	// The trajectory generation function
	Point& TrajFun(float cur_length);

	// Get the line's slant angle
	Angle& SlantAngle();

	// The curvature generation function
	float CurvatureFun() const;
	
 public:
	Point start_point_;  // The start point
	Point end_point_;    // The end point
	Point point_;        // Working point for trajectory generation
	Angle slant_angle_;  // Slant angle
	float total_length_; // The length of the line

 private:
	// Generate line segments based on two points
	void Create(const Point& start_point, const Point& end_point);

	// Generate line segments based on the inclination angle of a point and a line
	void Create(const Point& start_point, const Angle& slant_angle, float total_length);
};